
//  Copyright (C) 2009-2011 Luca Piccioni
// 
//  This program is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
// 
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
// 
//  You should have received a copy of the GNU General Public License
//  along with this program.  If not, see <http://www.gnu.org/licenses/>.

using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;

using Derm.Render;
using Derm.Simd;

namespace Derm
{
	/// <summary>
	/// Square matrix of 4x4 components.
	/// </summary>
	public class Matrix4x4 : Matrix
	{
		#region Constructors

		/// <summary>
		/// Square matrix constructor.
		/// </summary>
		public Matrix4x4() : base(4, 4)
		{

		}

		/// <summary>
		/// Complement square matrix constructor.
		/// </summary>
		/// <param name="m">
		/// A <see cref="Matrix"/>
		/// </param>
		/// <param name="c">
		/// A <see cref="System.UInt32"/>
		/// </param>
		/// <param name="r">
		/// A <see cref="System.UInt32"/>
		/// </param>
		public Matrix4x4(Matrix4x4 m, uint c, uint r) : base(m, c, r)
		{

		}

		/// <summary>
		/// Matrix copy constructor.
		/// </summary>
		/// <param name="m">
		/// A <see cref="Matrix"/>
		/// </param>
		public Matrix4x4(Matrix4x4 m) : base(m)
		{

		}

		#endregion

		#region Methods

		/// <summary>
		/// 
		/// </summary>
		/// <param name="matrices"></param>
		/// <returns></returns>
		public static Matrix4x4 Concatenate(params Matrix4x4[] matrices)
		{
			if (matrices.Length == 0)
				throw new ArgumentNullException("matrices", "no matric chain");

			Matrix4x4 m = new Matrix4x4();
			
			m.SetIdentity();

			unsafe {
				if (SimdLibrary.Matrix4x4_Concatenate != null) {
					List<GCHandle> gcHandles = new List<GCHandle>();
					GCHandle matrixData;

					try {
						float*[] matrixChain = new float*[matrices.Length + 1];

						matrixData = GCHandle.Alloc(m.MatrixBuffer, GCHandleType.Pinned);
						gcHandles.Add(matrixData);
						matrixChain[0] = (float*) matrixData.AddrOfPinnedObject().ToPointer();

						for (int i = 0; i < matrices.Length; i++) {
							matrixData = GCHandle.Alloc(matrices[i].MatrixBuffer, GCHandleType.Pinned);
							gcHandles.Add(matrixData);

							matrixChain[i + 1] = (float*) matrixData.AddrOfPinnedObject().ToPointer();
						}

						SimdLibrary.Matrix4x4_Concatenate(matrixChain, (uint) matrixChain.Length);

					} finally {
						foreach (GCHandle gcHandle in gcHandles)
							gcHandle.Free();	
					}
				} else {
					foreach (Matrix4x4 matrix in matrices)
						m = m * matrix;
				}
			}

			return (m);
		}

		#endregion

		#region Operators

		/// <summary>
		/// Compute the product of a Matrix4x4 with a Vertex3f.
		/// </summary>
		/// <param name="m">
		/// A <see cref="Matrix4x4"/> that specifies the left multiplication operand.
		/// </param>
		/// <param name="v">
		/// A <see cref="Vertex4f"/> that specifies the right vector operand.
		/// </param>
		/// <returns>
		/// A <see cref="Vertex3f"/> resulting from the product of the matrix <paramref name="m"/> and
		/// the vector <paramref name="v"/>. This operator is used to transform a vector by a matrix.
		/// </returns>
		public static Vertex4f operator *(Matrix4x4 m, Vertex3f v)
		{
			return (m * (Vertex4f)v);
		}

		/// <summary>
		/// Compute the product of a Matrix4x4 with a Vertex4f.
		/// </summary>
		/// <param name="m">
		/// A <see cref="Matrix4x4"/> that specifies the left multiplication operand.
		/// </param>
		/// <param name="v">
		/// A <see cref="Vertex4f"/> that specifies the right vector operand.
		/// </param>
		/// <returns>
		/// A <see cref="Vertex3f"/> resulting from the product of the matrix <paramref name="m"/> and
		/// the vector <paramref name="v"/>. This operator is used to transform a vector by a matrix.
		/// </returns>
		public static Vertex4f operator *(Matrix4x4 m, Vertex4f v)
		{
			Vertex4f result;

			result.x = m[0, 0] * v.x + m[1, 0] * v.y + m[2, 0] * v.z + m[3, 0] * v.w;
			result.y = m[0, 1] * v.x + m[1, 1] * v.y + m[2, 1] * v.z + m[3, 1] * v.w;
			result.z = m[0, 2] * v.x + m[1, 2] * v.y + m[2, 2] * v.z + m[3, 2] * v.w;
			result.w = m[0, 3] * v.x + m[1, 3] * v.y + m[2, 3] * v.z + m[3, 3] * v.w;

			return (result);
		}

		/// <summary>
		/// Compute the product of two Matrix4x4.
		/// </summary>
		/// <param name="m1">
		/// A <see cref="Matrix4x4"/> that specifies the left multiplication operand.
		/// </param>
		/// <param name="m2">
		/// A <see cref="Matrix4x4"/> that specifies the right multiplication operand.
		/// </param>
		/// <returns>
		/// A <see cref="Matrix4x4"/> resulting from the product of the matrix <paramref name="m1"/> and
		/// the matrix <paramref name="m2"/>. This operator is used to concatenate successive transformations.
		/// </returns>
		public static Matrix4x4 operator *(Matrix4x4 m1, Matrix4x4 m2)
		{
			Matrix4x4 prod = new Matrix4x4();

			// COmpute product
			ComputeMatrixProduct(prod, m1, m2);

			return (prod);
		}

		/// <summary>
		/// Rotate a ModelMatrix in three-dimensional space using Quaternion.
		/// </summary>
		/// <param name="m"></param>
		/// <param name="q"></param>
		/// <returns></returns>
		public static Matrix4x4 operator *(Matrix4x4 m, Quaternion q)
		{
			Matrix4x4 prod = new Matrix4x4();

			// COmpute product
			ComputeMatrixProduct(prod, m, (Matrix4x4) q);

			return (prod);
		}

		/// <summary>
		/// Compute the product of two Matrix4x4.
		/// </summary>
		/// <param name="result">
		/// A <see cref="Matrix4x4"/> that stores the matrix multiplication result.
		/// </param>
		/// <param name="m1">
		/// A <see cref="Matrix4x4"/> that specifies the left multiplication operand.
		/// </param>
		/// <param name="m2">
		/// A <see cref="Matrix4x4"/> that specifies the right multiplication operand.
		/// </param>
		protected static void ComputeMatrixProduct(Matrix4x4 result, Matrix4x4 m, Matrix4x4 n)
		{
			if (result == null)
				throw new ArgumentNullException("result");
			if (m == null)
				throw new ArgumentNullException("m");
			if (n == null)
				throw new ArgumentNullException("n");

			unsafe {
				fixed (float* prodFix = result.MatrixBuffer)
				fixed (float* pm = m.MatrixBuffer)
				fixed (float* pn = n.MatrixBuffer)
				{
					prodFix[0x0] = pm[0x0] * pn[0x0] + pm[0x4] * pn[0x1] + pm[0x8] * pn[0x2] + pm[0xC] * pn[0x3];
					prodFix[0x4] = pm[0x0] * pn[0x4] + pm[0x4] * pn[0x5] + pm[0x8] * pn[0x6] + pm[0xC] * pn[0x7];
					prodFix[0x8] = pm[0x0] * pn[0x8] + pm[0x4] * pn[0x9] + pm[0x8] * pn[0xA] + pm[0xC] * pn[0xB];
					prodFix[0xC] = pm[0x0] * pn[0xC] + pm[0x4] * pn[0xD] + pm[0x8] * pn[0xE] + pm[0xC] * pn[0xF];

					prodFix[0x1] = pm[0x1] * pn[0x0] + pm[0x5] * pn[0x1] + pm[0x9] * pn[0x2] + pm[0xD] * pn[0x3];
					prodFix[0x5] = pm[0x1] * pn[0x4] + pm[0x5] * pn[0x5] + pm[0x9] * pn[0x6] + pm[0xD] * pn[0x7];
					prodFix[0x9] = pm[0x1] * pn[0x8] + pm[0x5] * pn[0x9] + pm[0x9] * pn[0xA] + pm[0xD] * pn[0xB];
					prodFix[0xD] = pm[0x1] * pn[0xC] + pm[0x5] * pn[0xD] + pm[0x9] * pn[0xE] + pm[0xD] * pn[0xF];

					prodFix[0x2] = pm[0x2] * pn[0x0] + pm[0x6] * pn[0x1] + pm[0xA] * pn[0x2] + pm[0xE] * pn[0x3];
					prodFix[0x6] = pm[0x2] * pn[0x4] + pm[0x6] * pn[0x5] + pm[0xA] * pn[0x6] + pm[0xE] * pn[0x7];
					prodFix[0xA] = pm[0x2] * pn[0x8] + pm[0x6] * pn[0x9] + pm[0xA] * pn[0xA] + pm[0xE] * pn[0xB];
					prodFix[0xE] = pm[0x2] * pn[0xC] + pm[0x6] * pn[0xD] + pm[0xA] * pn[0xE] + pm[0xE] * pn[0xF];

					prodFix[0x3] = pm[0x3] * pn[0x0] + pm[0x7] * pn[0x1] + pm[0xB] * pn[0x2] + pm[0xF] * pn[0x3];
					prodFix[0x7] = pm[0x3] * pn[0x4] + pm[0x7] * pn[0x5] + pm[0xB] * pn[0x6] + pm[0xF] * pn[0x7];
					prodFix[0xB] = pm[0x3] * pn[0x8] + pm[0x7] * pn[0x9] + pm[0xB] * pn[0xA] + pm[0xF] * pn[0xB];
					prodFix[0xF] = pm[0x3] * pn[0xC] + pm[0x7] * pn[0xD] + pm[0xB] * pn[0xE] + pm[0xF] * pn[0xF];
				}
			}
		}

		#endregion

		#region Matrix Overrides

		/// <summary>
		/// Clone this Matrix4x4.
		/// </summary>
		/// <returns>
		/// It returns a deep copy of this Matrix4x4.
		/// </returns>
		public override Matrix Clone()
		{
			return (new Matrix4x4(this));
		}

		/// <summary>
		/// Inverse Matrix of this Matrix.
		/// </summary>
		/// <returns>
		/// A <see cref="Matrix"/> representing the inverse matrix of this Matrix.
		/// </returns>
		/// <exception cref="InvalidOperationException">
		/// The exception is thrown if this Matrix is not square, or it's determinant is 0.0.
		/// </exception>
		public new Matrix4x4 GetInverseMatrix()
		{
			float px = Math.Abs(this[0, 3]), py = Math.Abs(this[1, 3]), pz = Math.Abs(this[2, 3]), ps = Math.Abs(this[3, 3] - 1.0f);

			if ((px > Single.Epsilon) || (py > Single.Epsilon) || (pz > Single.Epsilon) || (ps > Single.Epsilon) || true)
				return ((Matrix4x4)base.GetInverseMatrix());	// Most general case, but rare
			else {
				Matrix3x3 rotMatrix = new Matrix3x3(this, 3, 3);
				Vertex3f posMatrix;

				Matrix4x4 inverseMatrix = (Matrix4x4) Clone();

				// Invert rotation matrix
				rotMatrix = rotMatrix.GetInverseMatrix();
				
				unsafe {
					fixed (float* src = rotMatrix.MatrixBuffer)
					fixed (float* dst = inverseMatrix.MatrixBuffer) {

						// Copy rotation matrix into the inverse
						dst[0] = src[0]; dst[4] = src[3]; dst[8]  = src[6];
						dst[1] = src[1]; dst[5] = src[4]; dst[9]  = src[7];
						dst[2] = src[2]; dst[6] = src[5]; dst[10] = src[8];
						dst[3] = 0.0f;   dst[7] = 0.0f;   dst[11] = 0.0f;   dst[15] = 1.0f;
					}

					fixed (float* src = MatrixBuffer)
					fixed (float* dst = inverseMatrix.MatrixBuffer) {

						// Negate translation
						dst[12] = -src[12];
						dst[13] = -src[13];
						dst[14] = -src[14];

					}
				}

				return (inverseMatrix);
			}
		}

		#endregion
	}
}
